﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using GestureSaver;
using Microsoft.Kinect;
using CubeRotation;

namespace Experimenter
{
    public static class Common
    {
        public static JointType[] SelectedJoints = { JointType.HandLeft, JointType.WristLeft, JointType.ElbowLeft, JointType.HandRight, JointType.WristRight, JointType.ElbowRight };

        public static void RectificateAllProjectiveMotionDistortion(GestureDB gestureDB)
        {
            foreach (List<Gesture> gestureClass in gestureDB)
            {
                foreach (Gesture gestureSample in gestureClass)
                {
                    // for all gestureSamples ...

                    RectificateProjectiveMotionDistortion(gestureSample);
                }
            }
        }

        public static void RectificateProjectiveMotionDistortion(Gesture g)
        {
            Rectificater.Rectificate(g);
        }

        public static void CenterizeAllGestures(GestureDB gestureDB)
        {
            foreach (List<Gesture> gestureClass in gestureDB)
            {
                foreach (Gesture gestureSample in gestureClass)
                {
                    // for all gestureSamples ...

                    CenterizeGesture(gestureSample);                
                }
            }
        }

        public static void CenterizeGesture(Gesture gestureSample)
        {
            foreach (MSkeleton skel in gestureSample)
            {
                MJoint shoulderCenter = DeepCloner.DeepClone<MJoint>(skel[JointType.ShoulderCenter]);

                foreach (MJoint j in skel)
                {
                    j.Position.X -= shoulderCenter.Position.X;
                    j.Position.Y -= shoulderCenter.Position.Y;
                    j.Position.Z -= shoulderCenter.Position.Z;
                }
            }              
        }

        public static void NormalizeAllShoulderLengths(GestureDB gestureDB)
        {
            foreach (List<Gesture> gestureClass in gestureDB)
            {
                foreach (Gesture gestureSample in gestureClass)
                {
                    // for all gestureSamples ...

                    NormalizeShoulderLength(gestureSample);                  
                }
            }
        }

        public static float GetShoulderLength(Gesture gestureSample)
        {
            float shoulderLength = 0;

            foreach (MSkeleton skel in gestureSample)
            {
                shoulderLength += (float)Math.Sqrt(Math.Pow(skel[JointType.ShoulderLeft].Position.X - skel[JointType.ShoulderRight].Position.X, 2) +
                                                   Math.Pow(skel[JointType.ShoulderLeft].Position.Y - skel[JointType.ShoulderRight].Position.Y, 2));
            }

            shoulderLength /= gestureSample.SkeletonCount;

            return shoulderLength;
        }

        public static void NormalizeShoulderLength(Gesture gestureSample)
        {
            float shoulderLength = GetShoulderLength(gestureSample);

            foreach (MSkeleton skel in gestureSample)
            {
                foreach (MJoint j in skel)
                {
                    j.Position.X /= shoulderLength;
                    j.Position.Y /= shoulderLength;
                    j.Position.Z /= shoulderLength;
                }
            }           
        }    

        // Compute the min DTW distance between seq2 and all possible endings of seq1.
        public static double DTW(Gesture g1, Gesture g2, JointType[] selectedJoints = null, Dictionary<JointType, double> weights = null, int maxPenalty = 20)
        {        
            if (selectedJoints == null)
                selectedJoints = SelectedJoints;

            double[,] costs = new double[g1.SkeletonCount, g2.SkeletonCount];
            int[,] horizontalPenalty = new int[g1.SkeletonCount, g2.SkeletonCount];
            int[,] verticalPenalty = new int[g1.SkeletonCount, g2.SkeletonCount];

            for (int i = 0; i < g1.SkeletonCount; i++)
                for (int j = 0; j < g2.SkeletonCount; j++)
                    costs[i, j] = double.PositiveInfinity;

            costs[0, 0] = Distance(g1[0], g2[0], selectedJoints, weights);

            for (int i = 1; i < g1.SkeletonCount; i++) // first column
            {
                if (verticalPenalty[i - 1, 0] < maxPenalty)
                {
                    costs[i, 0] = costs[i - 1, 0] + Distance(g1[i], g2[0], selectedJoints, weights);
                    verticalPenalty[i, 0] = verticalPenalty[i - 1, 0] + 1;
                }
                else
                {
                    break;
                }
            }

            for (int i = 1; i < g2.SkeletonCount; i++) // first row
            {
                if (horizontalPenalty[0, i - 1] < maxPenalty)
                {
                    costs[0, i] = costs[0, i - 1] + Distance(g1[0], g2[i], selectedJoints, weights);
                    horizontalPenalty[0, i] = horizontalPenalty[0, i - 1] + 1;
                }
                else
                {
                    break;
                }
            }

            // horizontal, vertical
            double[] tempCosts = new double[2];

            for (int i = 1; i < g1.SkeletonCount; i++)
            {
                for (int j = 1; j < g2.SkeletonCount; j++)
                {
                    double newCost = Distance(g1[i], g2[j], selectedJoints, weights);

                    for (int p = 0; p < 2; p++)
                        tempCosts[p] = Double.PositiveInfinity;

                    if (horizontalPenalty[i - 1, j] < maxPenalty)
                        tempCosts[0] = costs[i - 1, j];
                    if (verticalPenalty[i, j - 1] < maxPenalty)
                        tempCosts[1] = costs[i, j - 1];

                    double oldCost = costs[i - 1, j - 1];

                    int minIndex = -1;

                    for (int p = 0; p < 2; p++)
                        if (tempCosts[p] < oldCost)
                            minIndex = p;

                    switch (minIndex)
                    {
                        case -1: // diagonal
                            oldCost = costs[i - 1, j - 1];
                            horizontalPenalty[i, j] = 0;
                            verticalPenalty[i, j] = 0;
                            break;
                        case 0: // horizontal
                            oldCost = costs[i - 1, j];
                            horizontalPenalty[i, j] = horizontalPenalty[i - 1, j] + 1;
                            verticalPenalty[i, j] = 0;
                            break;
                        case 1: // vertical 
                            oldCost = costs[i, j - 1];
                            verticalPenalty[i, j] = verticalPenalty[i, j - 1] + 1;
                            horizontalPenalty[i, j] = 0;
                            break;
                        default:
                            throw new Exception("Something is wrong");
                    }

                    costs[i, j] = oldCost + newCost;
                }
            }

            // Find best between seq2 and an ending (postfix) of seq1.
            double bestMatch = double.PositiveInfinity;

            for (int i = (g1.SkeletonCount*3) / 4 ; i < g1.SkeletonCount; i++)
                if (costs[i, g2.SkeletonCount - 1] < bestMatch)
                    bestMatch = costs[i, g2.SkeletonCount - 1];

            for (int i = (g2.SkeletonCount*3) / 4 + 15; i < g2.SkeletonCount; i++)
                if (costs[g1.SkeletonCount - 1, i] < bestMatch)
                    bestMatch = costs[g1.SkeletonCount - 1, i];

            if (Double.IsInfinity(bestMatch))
            {
                if (maxPenalty >= 30)
                {
                    return 280;
                }

                return DTW(g1, g2, selectedJoints, weights, maxPenalty + 10);
            }

            return bestMatch;
        }

        // Computes a 2-distance between two observations. (aka Euclidian distance).
        public static double Distance(MSkeleton s1, MSkeleton s2, JointType[] selectedJoints, Dictionary<JointType, double> weights = null)
        {
            double d = 0;

            foreach (JointType j in selectedJoints)
            {
                double euclideanDistance = EuclideanDistance(s1[j], s2[j]);

                if (weights == null)
                {
                    d += euclideanDistance;
                }
                else // weighted DTW
                {
                    d += euclideanDistance * weights[j];
                }                
            }

            if (weights != null)
            {
                d = d * weights.Count;
            }
            
            return d;
        }

        public static double EuclideanDistance(MJoint j1, MJoint j2)
        {
            return Math.Sqrt(Math.Pow(j1.Position.X - j2.Position.X, 2) + Math.Pow(j1.Position.Y - j2.Position.Y, 2));
        }        
        
        public static void NormalizeWeights(Dictionary<JointType, double> weights)
        {
            double sum = 0;

            foreach (JointType j in SelectedJoints)
            {
                sum += weights[j];
            }

            foreach (JointType j in SelectedJoints)
            {
                weights[j] = weights[j] / sum;
            }
        }

        public static Dictionary<JointType, double> GetTotalDisplacements(Gesture g)
        {
            Dictionary<JointType, double> rVal = new Dictionary<JointType, double>();

            foreach (JointType j in Common.SelectedJoints)
            {
                rVal[j] = 0;
            }

            for (int i = 0; i < g.SkeletonCount - 1; i++)
            {
                foreach (JointType j in Common.SelectedJoints)
                {
                    double dist = Common.EuclideanDistance(g[i][j], g[i + 1][j]);
                    rVal[j] += dist;
                }
            }

            return rVal;
        }

        public static Dictionary<GestureType, Dictionary<JointType, double>> GetTotalDisplacements(GestureDB gestureDB)
        {
            Dictionary<GestureType, Dictionary<JointType, double>> rVal = new Dictionary<GestureType, Dictionary<JointType, double>>();

            for (int i = 0; i < gestureDB.TotalClassCount; i++)
            {
                Dictionary<JointType, double> weights = GetTotalDisplacements(gestureDB[i][0]);

                for (int j = 1; j < gestureDB[i].Count; j++)
                {
                    Dictionary<JointType, double> temp = GetTotalDisplacements(gestureDB[i][j]);

                    foreach (JointType joint in Common.SelectedJoints)
                    {
                        weights[joint] += temp[joint];
                    }
                }

                Common.NormalizeWeights(weights);

                rVal.Add(gestureDB[i][0].gestureType, weights);
            }

            return rVal;
        }

    }
}
